﻿using System.Runtime.CompilerServices;

namespace RobotPerCeption.utils
{
    public class RoboteUtils
    {

        public static int GJZB = 0;

        public static int ZJZB = 0;

        public static int GONGJUZB = 0;

        public static int YHZB = 0;

        public static double[]  jointRadToAngle(double[] positions, int positionIndex)
        {
            //0：关节 1：直角 2：工具 3：用户
            //关节坐标系 全改   其他模式 改 a b c

            switch (positionIndex)
            {
                case 0:
                    {
                        for (int i = 0; i < positions.Length; i++) {
                            positions[i] = RoboteUtils.radToAngle(positions[i]);                       
                        }
                        break;
                    }
                case 1:
                case 2:
                case 3:
                    {
                        positions[3] = RoboteUtils.radToAngle(positions[3]);
                        positions[3] = RoboteUtils.radToAngle(positions[4]);
                        positions[3] = RoboteUtils.radToAngle(positions[5]);
                        break;
                    }
            }

            return positions;
            //return rad * (180 / Math.PI);
        }
        /// <summary>
        /// 弧度转换为角度
        /// </summary>
        /// <param name="rad">弧度值</param>
        /// <returns></returns>
        private static double radToAngle(double rad)
        {
            return rad * (180 / Math.PI);
        }

       
    }
}
